#include "TriEdge.h"
#include "TriFace.h"
#include "TriVertex.h"
#include "TriMesh.h"

#include <iostream>
using namespace std;

#ifdef _DEBUG
static const Real REL_TOL1 = 1e-7f;
static const Real REL_TOL2 = 1e-6f;
static const Real REL_TOL_SQRT = 1e-4f;
#endif


//////////////////////////////////////////////////////////////////////////////
// TriEdge functions
//////////////////////////////////////////////////////////////////////////////

void TriEdge::Initialize(void)
{
   id=-1;
   marked=false;
   feature=false;
   degenerate=NONE;
   u=Vector3(1.0, 0.0, 0.0);
   len0=0.0;
   next=NULL;
   twin=NULL;
   orig=NULL;
   face=NULL;
   owner=NULL;
}

#ifdef _DEBUG
bool TriEdge::VerifyTopology( int pos1, int pos2 ) const
{
    bool result = true;

    if( orig == NULL ) {
        cerr << "TriEdge at position " << pos1 << ", " << pos2
             << " does not have vertex" << endl;
        result = false;
    }

    if( face == NULL ) {
        cerr << "TriEdge at position " << pos1 << ", " << pos2
             << " does not have face" << endl;
        result = false;
    } else if( face->Edge1() != this && face->Edge2() != this &&
               face->Edge3() != this
    ) {
        cerr << "TriEdge at position " << pos1 << ", " << pos2
             << " does not point to the face that points to it" << endl;
        result = false;
    }

    if( next == NULL ) {
        cerr << "TriEdge at position " << pos1 << ", " << pos2
             << " does not have next" << endl;
        result = false;
    } else if( next->Next()->Next() != this ) {
        cerr << "TriEdge at position " << pos1 << ", " << pos2
             << " does not point to next correctly" << endl;
        result = false;
    }


    if( Marked() ) {
        cerr << "TriEdge at position " << pos1 << ", " << pos2
             << " is marked" << endl;
    }

    return result;
}

// The tests are formulated so as to fail if any of the numbers are NAN
// Since NAN (compare) x = FALSE for all x and all comparison operators.
bool TriEdge::VerifyGeometry( int pos1, int pos2 ) const
{
    bool result = true;

    if( twin != NULL ) {
        if( !(len0 == twin->LengthInit()) ) {
            cerr << "TriEdge at position " << pos1 << ", " << pos2
                 << " does not have twin len" << endl;
            result = false;
        }

        //if( !((u - twin->Direction()) ) {
        //    cerr << "TriEdge at position " << pos1 << ", " << pos2
        //         << " does not have twin -u" << endl;
        //    result = false;
        //}
    }

    // Make sure that the direction and length match with the set endpoints
    Vector3 tempu = u;
    Vector3 temp1 = next->Origin()->PositionInit() - orig->PositionInit();
    double lsq = temp1.getSquaredLength();
    double length_sq = len0 * len0;

    if( !(length_sq / lsq < 1.0 + REL_TOL1 && length_sq / lsq > 1.0 - REL_TOL1)
    ) {
        cerr << "TriEdge at position " << pos1 << ", " << pos2 << " length = "
             << len0 << " does not match with endpoints length = "
             << sqrt( lsq ) << endl;
        result = false;
    }

    if( len0 < REL_TOL2 ) {
        cerr << "TriEdge at position " << pos1 << ", " << pos2 << " length = "
             << len0 << " is too short!" << endl;
        result = false;
    }

    tempu.normalize();
    temp1.normalize();
    length_sq = Vector3::dotProd(tempu, temp1);

    if( !(length_sq < 1.0 + REL_TOL2 && length_sq > 1.0 - REL_TOL2) ) {
        cerr << "TriEdge at position " << pos1 << ", " << pos2 << 
           " does not match with endpoints direction" << endl;
        result = false;
    }

    // Attempt to make sure that the edge direction vector is unit length
    if( !(u.getSquaredLength() < 1.0 + REL_TOL2 && u.getSquaredLength() > 1.0 - REL_TOL2)
    ) {
        cerr << "TriEdge at position " << pos1 << ", " << pos2
             << " does not have a unit length u vector" << endl;
        result = false;
    }

    return result;
}
#endif

bool TriEdge::IsAdjacentTo(const TriFace *other) const
{
   return other->Contains(Origin()) || other->Contains(Head());
}

void TriEdge::ComputeDirectionLength(void)
{
   u=next->Origin()->Position()-orig->Position();
   len0=u.getLength();
   u*=(1.0/len0);
}

void TriEdge::ComputeDirectionLengthTwin(void)
{
   ComputeDirectionLength();
   twin->SetLengthInit(len0);
   twin->SetDirection(-u);
}

void TriEdge::ComputeConvexity(void)
{
   if(Twin()==NULL)
   {
      reflex=true;
   }
   else
   {
      reflex = Vector3::dotProd(AdjFace()->Normal(), Twin()->AdjFace()->Normal()) < 0.0;
   }
}

void TriEdge::ComputeConvexityTwin(void)
{
   ComputeConvexity();
   Twin()->SetConvexity(reflex);
}

void TriEdge::ComputeGeometry(void)
{
   ComputeDirectionLength();
   ComputeConvexity();
}

void TriEdge::ComputeGeometryTwins(void)
{
   if(!Marked())
   {
      if(Twin()!=NULL)
      {
         ComputeDirectionLengthTwin();
         ComputeConvexityTwin();
         Twin()->Mark();
      }
      else
      {
         ComputeDirectionLength();
         ComputeConvexity();
      }
   }
   else
   {
      Unmark();
   }
}

void TriEdge::ComputeArea(void)
{
   if (Twin() != NULL)
   {
      area0 = 0.5 * (AdjFace()->getArea() + Twin()->AdjFace()->getArea());
   }
}

Real TriEdge::Length(void) const
{
   return (Origin()->Position() - Head()->Position()).getLength();
}

void TriEdge::CheckBoundary(void)
{
   if(Twin()==NULL)
   {
      Origin()->SetBoundary();
      Head()->SetBoundary();
   }
}

bool TriEdge::IsFixed(void) const
{
   return Origin()->IsFixed() && Head()->IsFixed();
}


Real TriEdge::MaxCollisionTime(void) const
{
   return max(Origin()->CollisionTime(), Head()->CollisionTime());
}

